/********************************************************************************
 * Copyright 2009 The Robotics Group, The Maersk Mc-Kinney Moller Institute,
 * Faculty of Engineering, University of Southern Denmark
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *     http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 ********************************************************************************/

#ifndef RWLIBS_ALGORITHMS_BASICGPM_HPP
#define RWLIBS_ALGORITHMS_BASICGPM_HPP

#include <rw/kinematics/State.hpp>
#include <rw/math/Q.hpp>
#include <rw/math/VelocityScrew6D.hpp>

namespace rw { namespace models {
    class Device;
}}    // namespace rw::models

namespace rwlibs { namespace algorithms {

    /**
     * @brief Implements a Gradient Projection Method (GPM)
     *
     * GPM implements the basic Gradient Projection Method with joint limit and
     * singularity avoidance presented in [1]: "Using the task function approach to
     * avoid robot joint limits and kinematic singularities in visual servoing"
     * by Marchand, Chaumette and Rizzo, IEEE/RSJ Int. Conf. on Intelligent Robots
     * and Systems, 1996, vol. 3, pp. 1083-1090.
     *
     *
     */
    class BasicGPM
    {
      public:
        /**
         * @brief Constructs BasicGPM object
         *
         * @param device [in] Device to work with
         * @param controlFrame [in] The frame on the device to control. Usually this will be the
         * tool, but not necessarily.
         * @param state [in] Default state
         * @param qhome [in] Configuration somewhere between the lower and upper limit and towards
         * which the joints should move
         * @param dt [in] Step size
         */
        BasicGPM (rw::models::Device* device, rw::kinematics::Frame* controlFrame,
                  const rw::kinematics::State& state, const rw::math::Q& qhome, double dt);

        /**
         * @brief Destructor
         */
        virtual ~BasicGPM ();

        /**
         * @brief Solves for joint velocities given a desired tool velocity
         *
         * @param q [in] The current joint configuration
         * @param dq [in] The current joint velocity
         * @param tcpvel [in] The desired tool velocity seen in the base frame
         */
        rw::math::Q solve (const rw::math::Q& q, const rw::math::Q& dq,
                           const rw::math::VelocityScrew6D<>& tcpvel);

        /**
         * @brief Specifies whether to use joint limit avoidance
         *
         * The method implements the method with Activation threshold from the [1]
         *
         * @param use [in] True to use joint limit avoidance
         */
        void setUseJointLimitsCost (bool use);

        /**
         * @brief Specifies whether to use singularity avoidance
         *
         * The singularity avoidance as described in [1]
         *
         * @param use [in] True to use singularity avoidance
         */
        void setUseSingularityCost (bool use);

        /**
         * @brief Sets the weight of the joint limits
         *
         * @param w [in] Weight of the joint limit
         */
        void setJointLimitsWeight (double w);

        /**
         * @brief Sets the threshold for the joint limits
         *
         * Given an upper and a lower bound \f$upper\f$ and \f$lower\f$ the proximity of the joint
         * limits is defined as \f$upper-\tau_{max} (upper-lower)\f$ and \f$lower+\tau_{min}
         * (upper-lower)\f$ where \f$\tau_{min}\f$ and \f$\tau_{max}\f$ are the thresholds specified
         * here.
         *
         * @param thresholdLowerRatio [in] Relative threshold for the lower joint limits
         * @param thresholdUpperRatio [in] Relative threshold for the upper joint limits
         */
        void setJointLimitThreshold (double thresholdLowerRatio, double thresholdUpperRatio);

        /**
         * @brief Sets the weight of the singularity avoidance task
         *
         * @param w [in] The weight
         */
        void setSingularityWeight (double w);

        /**
         * @brief Enumeration used to specify frame associated with the projection
         */
        enum ProjectionFrame {
            BaseFrame = 0, /** Robot Base Frame */
            ControlFrame   /**The Frame specified as the controlFrame*/
        };

        /**
         * @brief Setup the projection
         *
         * The traditional relationship between device Jacobian, joint velocities and tool velocity
         * is given by \f$J\dot{q}=\dot{q}\f$. To ignore certain degrees of freedom or put more
         * emphasis (with respect to the least square solution) on some we can multiply with
         * \f$P\f$ to get \f$P J\dot{q}=P \dot{x}\f$.
         *
         * \f$P\f$ needs to have exactly 6 columns, however the number of row may be less than 6.
         * Use the \b space flag to specify in which space the projection should occur.
         *
         * Usage: Setup to ignore tool roll
         * \code
         * BasicGPM* gpm = new BasicGPM(device, device->getEnd(), state, qhome, dt)
         * Eigen::MatrixXd P = Eigen::MatrixXd::Zero(5,6);
         * for (int i = 0; i<5; i++)
         *     P(i,i) = 1;
         * gpm->setProjection(P, BasicGPM::ControlFrame);
         * \endcode
         *
         * @param P [in] The projection matrix
         * @param space [in] The space in which to apply the projection
         */
        void setProjection (const Eigen::MatrixXd& P, ProjectionFrame space);

      private:
        rw::models::Device* _device;
        rw::kinematics::Frame* _controlFrame;
        rw::kinematics::State _state;
        rw::math::Q _qhome;
        int _dof;
        double _dt, _delta;
        rw::math::Q _qlower;
        rw::math::Q _qupper;
        rw::math::Q _dqlimit;
        rw::math::Q _ddqlimit;

        Eigen::MatrixXd _P;
        ProjectionFrame _space;

        // TODO Rewrite into using the PropertyMap
        bool _useJointLimitsCost;
        bool _useSingularityCost;

        double _weightSingularity;
        double _weightJointLimits;

        rw::math::Q _thresholdLower;
        rw::math::Q _thresholdUpper;

        Eigen::VectorXd getCostGradient (const rw::math::Q& q, const Eigen::MatrixXd& jac);

        rw::math::Q applyJointVelocityConstraint (const rw::math::Q& q, const rw::math::Q& dq,
                                                  const rw::math::Q& dqnew);

        void calculatePosAndVelLimits (Eigen::VectorXd& lower, Eigen::VectorXd& upper,
                                       const rw::math::Q& q, const rw::math::Q& dq);
    };

}}    // namespace rwlibs::algorithms

#endif /*RWLIBS_ALGORITHMS_BASICGPM_HPP*/
